#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2022/1/7 14:12
# @Author  : HYCX-AI-LAB Liang jinhao

"""
"""
"""
相机相关接口
"""
import ctypes
import sys
import time
import datetime
import numpy
import PIL
import matplotlib.pyplot as plt
import json

CAMERA_CFG = './utils/camera/CAMERA_CFG.json'
with open(CAMERA_CFG, 'r') as f:
    config = json.load(f)
    CAMERA_IP = config['CAMERA_IP']
    DATA_DIR = config['DATA_DIR']
    DATA_BACKUP_DIR = config['DATA_BACKUP_DIR']

deviceId = 0  # Set "0" if you use only 1 head.

import utils.camera.KEYENCE_LJX8000A.PYTHON.LJXAwrap as LJXAwrap
import time


def ethernet_set():
    ethernetConfig = LJXAwrap.LJX8IF_ETHERNET_CONFIG()
    ip = list(map(int, CAMERA_IP.split('.')))
    ethernetConfig.abyIpAddress[0] = ip[0]  # IP address
    ethernetConfig.abyIpAddress[1] = ip[1]
    ethernetConfig.abyIpAddress[2] = ip[2]
    ethernetConfig.abyIpAddress[3] = ip[3]
    ethernetConfig.wPortNo = 24691  # Port No.
    return ethernetConfig


def connect_camera():
    ethernetConfig = ethernet_set()
    res = LJXAwrap.LJX8IF_EthernetOpen(deviceId, ethernetConfig)
    print("LJXAwrap.LJX8IF_EthernetOpen:", hex(res))
    if res != 0:
        print("Failed to connect contoller.")
        return False
    return True


def close_camera():
    deviceId = 0
    res = LJXAwrap.LJX8IF_CommunicationClose(deviceId)
    print("LJXAwrap.LJX8IF_CommunicationClose:", hex(res))
    return


def change_programNo(programNo_set, camera_multiprocess_queue):
    if connect_camera() != True:
        res = {
            "ori_programNo_set": False,
            "activate_programNo_set": False,
            "change_status": -1
        }
        camera_multiprocess_queue.put(res)
        return
    # Get active program No.
    programNo_get = ctypes.c_ubyte()
    res = LJXAwrap.LJX8IF_GetActiveProgram(deviceId, programNo_get)
    if res != 0:
        print("LJX8IF_GetActiveProgram Error")
        res = {
            "ori_programNo_set": False,
            "activate_programNo_set": False,
            "change_status": -1
        }
        camera_multiprocess_queue.put(res)
        return
    print("LJXAwrap.LJX8IF_GetActiveProgram:", hex(res),
          "<ProgramNo_get>=", programNo_get.value)
    ori_programNo_set = programNo_get.value
    res = LJXAwrap.LJX8IF_ChangeActiveProgram(deviceId, programNo_set)
    if res != 0:
        print("Change Pro No Error")
        res = {
            "ori_programNo_set": ori_programNo_set,
            "activate_programNo_set": False,
            "change_status": -1
        }
        camera_multiprocess_queue.put(res)
        return
    print("LJXAwrap.LJX8IF_ChangeActiveProgram:", hex(res),
          "<ProgramNo_set>=", programNo_set)
    time.sleep(0.5)
    res = LJXAwrap.LJX8IF_GetActiveProgram(deviceId, programNo_get)
    if res != 0:
        print("LJX8IF_GetActiveProgram Error")
        res = {
            "ori_programNo_set": ori_programNo_set,
            "activate_programNo_set": False,
            "change_status": -1
        }
        camera_multiprocess_queue.put(res)
        return
    print("LJXAwrap.LJX8IF_GetActiveProgram:", hex(res),
          "<ProgramNo_get>=", programNo_get.value)
    activate_programNo_set = programNo_get.value

    res = {
        "ori_programNo_set": ori_programNo_set,
        "activate_programNo_set": activate_programNo_set,
        "change_status": 1
    }
    camera_multiprocess_queue.put(res)
    return


image_available = False  # Flag to conrirm the completion of image acquisition.
ysize_acquired = 0  # Number of Y lines of acquired image.
z_val = []  # The buffer for height image.
lumi_val = []  # The buffer for luminance image.


def save_csv(xsize, ysize, ZUnit, file_name, backup_file_name):
    z_val_mm = [0.0] * xsize * ysize
    for i in range(xsize * ysize):
        # Conver Z data to the actual length in millimeters
        if z_val[i] == 0:  # invalid value
            z_val_mm[i] = numpy.nan
        else:
            # 'Simple array data' is offset to be unsigned 16-bit data.
            # Decode by subtracting 32768 to get a signed value.
            z_val_mm[i] = int(z_val[i]) - 32768  # decode
            z_val_mm[i] *= ZUnit.value / 100.0  # um
            z_val_mm[i] /= 1000.0  # mm

    z_val_mm = numpy.array(z_val_mm).reshape(ysize, xsize)
    file = file_name + '.npy'
    numpy.save(file, z_val_mm)
    print("save {} csv done".format(file))
    backup_file = backup_file_name + '.npy'
    numpy.save(backup_file, z_val_mm)


def save_png(xsize, ysize, profinfo, ZUnit, file_name, backup_file_name):
    fig = plt.figure(figsize=(4.0, 6.0))
    plt.subplots_adjust(hspace=0.5)

    # Height image display
    ax1 = fig.add_subplot(3, 1, 1)
    img1 = PIL.Image.new('I', (xsize, ysize))
    img1.putdata(list(map(int, z_val)))
    im_list1 = numpy.asarray(img1)

    ax1.imshow(im_list1,
               cmap='gray',
               vmin=0,
               vmax=65535,
               interpolation='none')

    plt.title("Height Image")

    # Luminance image display
    ax2 = fig.add_subplot(3, 1, 2)
    img2 = PIL.Image.new('I', (xsize, ysize))
    img2.putdata(list(map(int, lumi_val)))
    im_list2 = numpy.asarray(img2)

    ax2.imshow(im_list2,
               cmap='gray',
               vmin=0,
               vmax=1024,
               interpolation='none')

    plt.title("Luminance Image")

    # Height profile display
    ax3 = fig.add_subplot(3, 1, 3)
    sl = int(xsize * ysize_acquired / 2)  # the horizontal center profile

    x_val_mm = [0.0] * xsize
    z_val_mm = [0.0] * xsize
    for i in range(xsize):
        # Conver X data to the actual length in millimeters
        x_val_mm[i] = (profinfo.lXStart + profinfo.lXPitch * i) / 100.0  # um
        x_val_mm[i] /= 1000.0  # mm

        # Conver Z data to the actual length in millimeters
        if z_val[sl + i] == 0:  # invalid value
            z_val_mm[i] = numpy.nan
        else:
            # 'Simple array data' is offset to be unsigned 16-bit data.
            # Decode by subtracting 32768 to get a signed value.
            z_val_mm[i] = int(z_val[sl + i]) - 32768  # decode
            z_val_mm[i] *= ZUnit.value / 100.0  # um
            z_val_mm[i] /= 1000.0  # mm

    plotz_min = numpy.nanmin(z_val_mm)
    if numpy.isnan(plotz_min):
        plotz_min = -1.0
    else:
        plotz_min -= 1.0

    plotz_max = numpy.nanmax(z_val_mm)
    if numpy.isnan(plotz_max):
        plotz_max = 1.0
    else:
        plotz_max += 1.0

    plt.ylim(plotz_min, plotz_max)

    ax3.plot(x_val_mm, z_val_mm)

    plt.title("Height Profile")

    file = file_name + '.png'
    backup_file = backup_file_name + '.png'
    # save the plt
    plt.savefig(file, dpi=500, bbox_inches='tight')
    print("save {} png done".format(file))
    plt.savefig(backup_file, dpi=500, bbox_inches='tight')


def clear_memory():
    # Ethernet open
    ethernetConfig = ethernet_set()
    res = LJXAwrap.LJX8IF_EthernetOpen(0, ethernetConfig)
    print("LJXAwrap.LJX8IF_EthernetOpen:", hex(res))
    if res != 0:
        print("Failed to connect contoller.")
        print("Exit the program.")
        return False

    res_clear = LJXAwrap.LJX8IF_ClearMemory(deviceId)
    print('res_clear:', res_clear)

    # Close
    res = LJXAwrap.LJX8IF_CommunicationClose(deviceId)
    print("LJXAwrap.LJX8IF_CommunicationClose:", hex(res))
    return True


def get_camera_data(file_name, backup_file_name, timeout_sec, camera_multiprocess_queue):
    global image_available
    global ysize_acquired
    global z_val
    global lumi_val
    deviceId = 0  # Set "0" if you use only 1 head.
    ysize = 4000  # Number of Y lines.
    timeout_sec = timeout_sec  # Timeout value for the acquiring image
    use_external_batchStart = False  # 'True' if you start batch externally.
    HighSpeedPortNo = 24692  # Port No. for high-speed

    ethernetConfig = ethernet_set()
    # Ethernet open
    if connect_camera() != True:
        res = {
            'get_status': -1,
            'msg': 'connect error'
        }
        camera_multiprocess_queue.put(res)
        return

    # Initialize Hi-Speed Communication
    my_callback_s_a = LJXAwrap.LJX8IF_CALLBACK_SIMPLE_ARRAY(callback_s_a)

    res = LJXAwrap.LJX8IF_InitializeHighSpeedDataCommunicationSimpleArray(
        deviceId,
        ethernetConfig,
        HighSpeedPortNo,
        my_callback_s_a,
        ysize,
        0)
    print("LJXAwrap.LJX8IF_InitializeHighSpeedDataCommunicationSimpleArray:",
          hex(res))
    if res != 0:
        res = {
            'get_status': -1,
            'msg': 'LJX8IF_InitializeHighSpeedDataCommunicationSimpleArray Error'
        }
        camera_multiprocess_queue.put(res)
        return

    # PreStart Hi-Speed Communication
    req = LJXAwrap.LJX8IF_HIGH_SPEED_PRE_START_REQ()
    req.bySendPosition = 1
    profinfo = LJXAwrap.LJX8IF_PROFILE_INFO()

    res = LJXAwrap.LJX8IF_PreStartHighSpeedDataCommunication(
        deviceId,
        req,
        profinfo)
    print("LJXAwrap.LJX8IF_PreStartHighSpeedDataCommunication:", hex(res))
    if res != 0:
        res = {
            'get_status': -1,
            'msg': 'LJX8IF_PreStartHighSpeedDataCommunication Error'
        }
        camera_multiprocess_queue.put(res)
        return

    # allocate the memory
    xsize = profinfo.wProfileDataCount
    z_val = [0] * xsize * ysize
    lumi_val = [0] * xsize * ysize

    # Start Hi-Speed Communication
    image_available = False
    res = LJXAwrap.LJX8IF_StartHighSpeedDataCommunication(deviceId)
    print("LJXAwrap.LJX8IF_StartHighSpeedDataCommunication:", hex(res))
    if res != 0:
        res = {
            'get_status': -1,
            'msg': 'LJX8IF_StartHighSpeedDataCommunication Error'
        }
        camera_multiprocess_queue.put(res)
        return

    # Start Measure (Start Batch)
    if use_external_batchStart is False:
        res_StartMeasure = LJXAwrap.LJX8IF_StartMeasure(deviceId)
        print('LJXAwrap.LJX8IF_StartMeasure:', res_StartMeasure)

    # wait for the image acquisition complete
    start_time = time.time()
    while True:
        if image_available:
            break
        if time.time() - start_time > timeout_sec:
            break

    # Stop
    res = LJXAwrap.LJX8IF_StopHighSpeedDataCommunication(deviceId)
    print("LJXAwrap.LJX8IF_StoptHighSpeedDataCommunication:", hex(res))

    # Finalize
    res = LJXAwrap.LJX8IF_FinalizeHighSpeedDataCommunication(deviceId)
    print("LJXAwrap.LJX8IF_FinalizeHighSpeedDataCommunication:", hex(res))

    # Close
    res = LJXAwrap.LJX8IF_CommunicationClose(deviceId)
    print("LJXAwrap.LJX8IF_CommunicationClose:", hex(res))

    if image_available is not True:
        res = {
            'get_status': -1,
            'msg': 'Failed to acquire image (timeout)'
        }
        camera_multiprocess_queue.put(res)
        return

    ##################################################################
    # Information of the acquired image
    ##################################################################
    ZUnit = ctypes.c_ushort()
    LJXAwrap.LJX8IF_GetZUnitSimpleArray(deviceId, ZUnit)

    print("----------------------------------------")
    print(" Luminance output      : ", profinfo.byLuminanceOutput)
    print(" Number of X points    : ", profinfo.wProfileDataCount)
    print(" Number of Y lines     : ", ysize_acquired)
    print(" X pitch in micrometer : ", profinfo.lXPitch / 100.0)
    print(" Z pitch in micrometer : ", ZUnit.value / 100.0)
    print("----------------------------------------")

    if timeout_sec > 1:
        save_png(xsize, ysize, profinfo, ZUnit, file_name, backup_file_name)
        save_csv(xsize, ysize, ZUnit, file_name, backup_file_name)
    # close_camera()

    res = {
        'get_status': 1,
        'msg': 'get success'
    }
    camera_multiprocess_queue.put(res)

    return


def callback_s_a(p_header,
                 p_height,
                 p_lumi,
                 luminance_enable,
                 xpointnum,
                 profnum,
                 notify, user):
    global image_available
    global ysize_acquired
    global z_val
    global lumi_val

    print("notify is ", notify)
    print("image_available", image_available)

    if (notify == 0) or (notify == 0x10000) or (notify == 1):
        if profnum != 0:
            if image_available is False:
                for i in range(xpointnum * profnum):
                    z_val[i] = p_height[i]
                    if luminance_enable == 1:
                        lumi_val[i] = p_lumi[i]

                ysize_acquired = profnum
                image_available = True
    return
